2.5D Mapping, Pathfinding And Path Following In Uneven Terrain